#include <cstddef>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <string>
#include <fstream>
#include <vector>
#include <sstream>
#include <rerun.hpp>

using namespace std;



pcl::PointCloud<pcl::PointXYZI>::Ptr read_pcd(const string& globalmap_pcd)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());
    if (pcl::io::loadPCDFile(globalmap_pcd, *cloud) == -1) 
    { 
        cerr << "[ERROR] Could not read file: " << globalmap_pcd << endl;
        return nullptr;
    }
    else
    {
        cout << "MAP Loaded " << cloud->points.size() << endl;
    } 

    return cloud;
}

vector<rerun::Position3D> read_path(const string& waypoints_txt) 
{
    ifstream file(waypoints_txt);
    vector<rerun::Position3D> points;

    if (!file.is_open()) {
        cerr << "Error: Cannot open file " << waypoints_txt << endl;
        return points;
    }

    string line;
    while (getline(file, line)) 
    {
        stringstream ss(line);
        float x, y;
        if (ss >> x >> y) 
        {   
            // cout << "x: " << x << " y: " << y << endl;
            points.emplace_back(rerun::Position3D(x, y, 0));
        }
    }
    cout << "PATH Loaded " << points.size() << endl;
    file.close();
    return points;
}

bool map2rerun(const string& globalmap_pcd, rerun::RecordingStream& rec)
{
    auto pcd_clouds = read_pcd(globalmap_pcd);
    if(pcd_clouds == nullptr)
    {
        cerr << "can not open pcd file." << endl;
        return false;
    }

    std::vector<rerun::datatypes::Vec3D> points;
    std::vector<rerun::Color> colors; 

    for (const auto& pt : pcd_clouds->points) 
    {
        points.emplace_back(pt.x, pt.y, pt.z);
        colors.emplace_back(255, 0, 0);
    }

    rec.log("pcd_points", rerun::Points3D(points).with_colors(colors));

    return true;
}

bool waypoints2rerun(const string& waypoints_txt, rerun::RecordingStream& rec)
{
    auto path_points = read_path(waypoints_txt);
    if (path_points.empty()) {
        cerr << "can not open txt file." << endl;
        return false;
    }
    std::vector<rerun::LineStrip3D> line_strips = {rerun::LineStrip3D(path_points)};
    rec.log("path_points", rerun::LineStrips3D(line_strips).with_colors(0x00FF00FF));

    return true;
}

int main()
{
    string globalmap_pcd ="../data/pcd_map/indoor.pcd";
    string waypoints_txt = "../data/path/waypoints.txt";
    rerun::RecordingStream rec("pcd_viewer");
    rec.spawn().exit_on_failure();
    auto map_loader = map2rerun(globalmap_pcd, rec);
    if(map_loader)
    {
        cout << "[PCD MAP]SUCCESS to Rerun !! " << endl;
    }
    else   
    {
        cout << "[PCD MAP]Failed to load point cloud " << endl;
    }
    auto way_loader = waypoints2rerun(waypoints_txt, rec);
    if(way_loader)
    {
        cout << "[PATH]SUCCESS to Rerun !! " << endl;
    }
    else   
    {
        cout << "[PATH]Failed to load point cloud " << endl;
    }
    return 0;
}